/**
 * File: ScoringObject.cpp
 * Date: November 2011
 * Author: Dorian Galvez-Lopez
 * Description: functions to compute bow scores 
 * License: see the LICENSE.txt file
 */

#include <cfloat>

#include "DBoW2/TemplatedVocabulary.h"
#include "DBoW2/BowVector.h"

using namespace DBoW2;

// If you change the type of WordValue, make sure you change also the
// epsilon value (this is needed by the KL method)
const double GeneralScoring::LOG_EPS = std::log(DBL_EPSILON);

// ---------------------------------------------------------------------------

double L1Scoring::score(const BowVector& v1, const BowVector& v2) const {
  BowVector::const_iterator v1_it, v2_it;
  const BowVector::const_iterator v1_end = v1.end();
  const BowVector::const_iterator v2_end = v2.end();

  v1_it = v1.begin();
  v2_it = v2.begin();

  double score = 0;

  while (v1_it != v1_end && v2_it != v2_end) {
    const WordValue& vi = v1_it->second;
    const WordValue& wi = v2_it->second;

    if (v1_it->first == v2_it->first) {
      score += std::abs(vi - wi) - std::abs(vi) - std::abs(wi);
      // move v1 and v2 forward
      ++v1_it;
      ++v2_it;
    }
    else if (v1_it->first < v2_it->first) {
      // move v1 forward
      v1_it = v1.lower_bound(v2_it->first);
      // v1_it = (first element >= v2_it.id)
    }
    else {
      // move v2 forward
      v2_it = v2.lower_bound(v1_it->first);
      // v2_it = (first element >= v1_it.id)
    }
  }

  // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|)
  //     for all i | v_i != 0 and w_i != 0
  // (Nister, 2006)
  // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1}
  score = -score / 2.0;

  return score;
}

// ---------------------------------------------------------------------------

double L2Scoring::score(const BowVector& v1, const BowVector& v2) const {
  BowVector::const_iterator v1_it, v2_it;
  const BowVector::const_iterator v1_end = v1.end();
  const BowVector::const_iterator v2_end = v2.end();

  v1_it = v1.begin();
  v2_it = v2.begin();

  double score = 0;

  while (v1_it != v1_end && v2_it != v2_end) {
    const WordValue& vi = v1_it->second;
    const WordValue& wi = v2_it->second;

    if (v1_it->first == v2_it->first) {
      score += vi * wi;
      // move v1 and v2 forward
      ++v1_it;
      ++v2_it;
    }
    else if (v1_it->first < v2_it->first) {
      // move v1 forward
      v1_it = v1.lower_bound(v2_it->first);
      // v1_it = (first element >= v2_it.id)
    }
    else {
      // move v2 forward
      v2_it = v2.lower_bound(v1_it->first);
      // v2_it = (first element >= v1_it.id)
    }
  }

  // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) )
  //     for all i | v_i != 0 and w_i != 0 )
  // (Nister, 2006)
  if (score >= 1.0)
    score = 1.0;
  else
    score = 1.0 - std::sqrt(1.0 - score);

  return score;
}

// ---------------------------------------------------------------------------
// ---------------------------------------------------------------------------

double ChiSquareScoring::score(const BowVector& v1, const BowVector& v2)
    const {
  BowVector::const_iterator v1_it, v2_it;
  const BowVector::const_iterator v1_end = v1.end();
  const BowVector::const_iterator v2_end = v2.end();

  v1_it = v1.begin();
  v2_it = v2.begin();

  double score = 0;

  // all the items are taken into account

  while (v1_it != v1_end && v2_it != v2_end) {
    const WordValue& vi = v1_it->second;
    const WordValue& wi = v2_it->second;

    if (v1_it->first == v2_it->first) {
      // (v-w)^2/(v+w) - v - w = -4 vw/(v+w)
      // we move the -4 out
      if (vi + wi != 0.0)
        score += vi * wi / (vi + wi);

      // move v1 and v2 forward
      ++v1_it;
      ++v2_it;
    }
    else if (v1_it->first < v2_it->first) {
      // move v1 forward
      v1_it = v1.lower_bound(v2_it->first);
    }
    else {
      // move v2 forward
      v2_it = v2.lower_bound(v1_it->first);
    }
  }

  // this takes the -4 into account
  score = 2. * score;

  return score;
}

// ---------------------------------------------------------------------------

double KLScoring::score(const BowVector& v1, const BowVector& v2) const {
  BowVector::const_iterator v1_it, v2_it;
  const BowVector::const_iterator v1_end = v1.end();
  const BowVector::const_iterator v2_end = v2.end();

  v1_it = v1.begin();
  v2_it = v2.begin();

  double score = 0;

  // all the items or v are taken into account

  while (v1_it != v1_end && v2_it != v2_end) {
    const WordValue& vi = v1_it->second;
    const WordValue& wi = v2_it->second;

    if (v1_it->first == v2_it->first) {
      if (vi != 0 && wi != 0)
        score += vi * std::log(vi / wi);
      // move v1 and v2 forward
      ++v1_it;
      ++v2_it;
    }
    else if (v1_it->first < v2_it->first) {
      // move v1 forward
      score += vi * (std::log(vi) - LOG_EPS);
      ++v1_it;
    }
    else {
      // move v2_it forward, do not add any score
      v2_it = v2.lower_bound(v1_it->first);
      // v2_it = (first element >= v1_it.id)
    }
  }

  // sum rest of items of v
  for (; v1_it != v1_end; ++v1_it)
    if (v1_it->second != 0)
      score += v1_it->second * (std::log(v1_it->second) - LOG_EPS);

  return score;
}

// ---------------------------------------------------------------------------

double BhattacharyyaScoring::score(const BowVector& v1,
                                   const BowVector& v2) const {
  BowVector::const_iterator v1_it, v2_it;
  const BowVector::const_iterator v1_end = v1.end();
  const BowVector::const_iterator v2_end = v2.end();

  v1_it = v1.begin();
  v2_it = v2.begin();

  double score = 0;

  while (v1_it != v1_end && v2_it != v2_end) {
    const WordValue& vi = v1_it->second;
    const WordValue& wi = v2_it->second;

    if (v1_it->first == v2_it->first) {
      score += std::sqrt(vi * wi);
      // move v1 and v2 forward
      ++v1_it;
      ++v2_it;
    }
    else if (v1_it->first < v2_it->first) {
      // move v1 forward
      v1_it = v1.lower_bound(v2_it->first);
      // v1_it = (first element >= v2_it.id)
    }
    else {
      // move v2 forward
      v2_it = v2.lower_bound(v1_it->first);
      // v2_it = (first element >= v1_it.id)
    }
  }

  return score;
}

// ---------------------------------------------------------------------------

double DotProductScoring::score(const BowVector& v1,
                                const BowVector& v2) const {
  BowVector::const_iterator v1_it, v2_it;
  const BowVector::const_iterator v1_end = v1.end();
  const BowVector::const_iterator v2_end = v2.end();

  v1_it = v1.begin();
  v2_it = v2.begin();

  double score = 0;

  while (v1_it != v1_end && v2_it != v2_end) {
    const WordValue& vi = v1_it->second;
    const WordValue& wi = v2_it->second;

    if (v1_it->first == v2_it->first) {
      score += vi * wi;
      // move v1 and v2 forward
      ++v1_it;
      ++v2_it;
    }
    else if (v1_it->first < v2_it->first) {
      // move v1 forward
      v1_it = v1.lower_bound(v2_it->first);
      // v1_it = (first element >= v2_it.id)
    }
    else {
      // move v2 forward
      v2_it = v2.lower_bound(v1_it->first);
      // v2_it = (first element >= v1_it.id)
    }
  }

  return score;
}

// ---------------------------------------------------------------------------
